/*
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#ifndef __OCTREE_H__
#define __OCTREE_H__

#include <cassert>
#include <cstring>
#include <stdio.h>
#include <math.h>
#include "GeoCommon.h"
#include "Projections.h"
#include "ModelReader.h"
#include "MemoryAllocator.h"
#include "cubes.h"
#include "Queue.h"
#include "manifold_table.h"
#include "dualcon.h"

/**
 * Main class and structures for scan-convertion, sign-generation,
 * and surface reconstruction.
 *
 * @author Tao Ju
 */

/* Global defines */
// Uncomment to see debug information
// #define IN_DEBUG_MODE

// Uncomment to see more output messages during repair
// #define IN_VERBOSE_MODE

/* Set scan convert params */

#define EDGE_FLOATS 4

union Node;
struct LeafNode;

struct InternalNode {
  /* Initialized in Octree::BuildTable */
  static int numChildrenTable[256];
  static int childrenCountTable[256][8];
  static int childrenIndexTable[256][8];

  /* Bit N indicates whether child N exists or not */
  unsigned char has_child_bitfield;
  /* Bit N indicates whether child N is a leaf or not */
  unsigned char child_is_leaf_bitfield;

  /* Can have up to eight children */
  Node *children[0];

  /// Test if child is leaf
  int is_child_leaf(int index) const
  {
    return (child_is_leaf_bitfield >> index) & 1;
  }

  /// If child index exists
  int has_child(int index) const
  {
    return (has_child_bitfield >> index) & 1;
  }

  /// Get the pointer to child index
  Node *get_child(int count)
  {
    return children[count];
  }

  const Node *get_child(int count) const
  {
    return children[count];
  }

  /// Get total number of children
  int get_num_children() const
  {
    return numChildrenTable[has_child_bitfield];
  }

  /// Get the count of children
  int get_child_count(int index) const
  {
    return childrenCountTable[has_child_bitfield][index];
  }
  int get_child_index(int count)
  {
    return childrenIndexTable[has_child_bitfield][count];
  }
  const int *get_child_counts() const
  {
    return childrenCountTable[has_child_bitfield];
  }

  /// Get all children
  void fill_children(Node *children[8], int leaf[8])
  {
    int count = 0;
    for (int i = 0; i < 8; i++) {
      leaf[i] = is_child_leaf(i);
      if (has_child(i)) {
        children[i] = get_child(count);
        count++;
      }
      else {
        children[i] = NULL;
        leaf[i] = 0;
      }
    }
  }

  /// Sets the child pointer
  void set_child(int count, Node *chd)
  {
    children[count] = chd;
  }
  void set_internal_child(int index, int count, InternalNode *chd)
  {
    set_child(count, (Node *)chd);
    has_child_bitfield |= (1 << index);
  }
  void set_leaf_child(int index, int count, LeafNode *chd)
  {
    set_child(count, (Node *)chd);
    has_child_bitfield |= (1 << index);
    child_is_leaf_bitfield |= (1 << index);
  }
};

/**
 * Bits order
 *
 * Leaf node:
 * Byte 0,1(0-11): edge parity
 * Byte 1(4,5,6): mask of primary edges intersections stored
 * Byte 1(7): in flood fill mode, whether the cell is in process
 * Byte 2(0-8): signs
 * Byte 3,4: in coloring mode, the mask for edges
 * Byte 5: edge intersections(4 bytes per inter, or 12 bytes if USE_HERMIT)
 */
struct LeafNode /* TODO: remove this attribute once everything is fixed */ {
  unsigned short edge_parity : 12;
  unsigned short primary_edge_intersections : 3;

  /* XXX: maybe actually unused? */
  unsigned short in_process : 1;

  /* bitfield */
  char signs;

  int minimizer_index;

  unsigned short flood_fill;

  float edge_intersections[0];
};

/* Doesn't get directly allocated anywhere, just used for passing
   pointers to nodes that could be internal or leaf. */
union Node {
  InternalNode internal;
  LeafNode leaf;
};

/* Global variables */
extern const int edgemask[3];
extern const int faceMap[6][4];
extern const int cellProcFaceMask[12][3];
extern const int cellProcEdgeMask[6][5];
extern const int faceProcFaceMask[3][4][3];
extern const int edgeProcEdgeMask[3][2][5];
extern const int faceProcEdgeMask[3][4][6];
extern const int processEdgeMask[3][4];
extern const int dirCell[3][4][3];
extern const int dirEdge[3][4];

/**
 * Structures for detecting/patching open cycles on the dual surface
 */

struct PathElement {
  // Origin
  int pos[3];

  // link
  PathElement *next;
};

struct PathList {
  // Head
  PathElement *head;
  PathElement *tail;

  // Length of the list
  int length;

  // Next list
  PathList *next;
};

/**
 * Class for building and processing an octree
 */
class Octree {
 public:
  /* Public members */

  /// Memory allocators
  VirtualMemoryAllocator *alloc[9];
  VirtualMemoryAllocator *leafalloc[4];

  /// Root node
  Node *root;

  /// Model reader
  ModelReader *reader;

  /// Marching cubes table
  Cubes *cubes;

  /// Length of grid
  int dimen;
  int mindimen, minshift;

  /// Maximum depth
  int maxDepth;

  /// The lower corner of the bounding box and the size
  float origin[3];
  float range;

  /// Counting information
  int nodeCount;
  int nodeSpace;
  int nodeCounts[9];

  int actualQuads, actualVerts;

  PathList *ringList;

  int maxTrianglePerCell;
  int outType;  // 0 for OFF, 1 for PLY, 2 for VOL

  // For flood filling
  int use_flood_fill;
  float thresh;

  int use_manifold;

  float hermite_num;

  DualConMode mode;

 public:
  /**
   * Construtor
   */
  Octree(ModelReader *mr,
         DualConAllocOutput alloc_output_func,
         DualConAddVert add_vert_func,
         DualConAddQuad add_quad_func,
         DualConFlags flags,
         DualConMode mode,
         int depth,
         float threshold,
         float hermite_num);

  /**
   * Destructor
   */
  ~Octree();

  /**
   * Scan convert
   */
  void scanConvert();

  void *getOutputMesh()
  {
    return output_mesh;
  }

 private:
  /* Helper functions */

  /**
   * Initialize memory allocators
   */
  void initMemory();

  /**
   * Release memory
   */
  void freeMemory();

  /**
   * Print memory usage
   */
  void printMemUsage();

  /**
   * Methods to set / restore minimum edges
   */
  void resetMinimalEdges();

  void cellProcParity(Node *node, int leaf, int depth);
  void faceProcParity(Node *node[2], int leaf[2], int depth[2], int maxdep, int dir);
  void edgeProcParity(Node *node[4], int leaf[4], int depth[4], int maxdep, int dir);

  void processEdgeParity(LeafNode *node[4], int depths[4], int maxdep, int dir);

  /**
   * Add triangles to the tree
   */
  void addAllTriangles();
  void addTriangle(Triangle *trian, int triind);
  InternalNode *addTriangle(InternalNode *node, CubeTriangleIsect *p, int height);

  /**
   * Method to update minimizer in a cell: update edge intersections instead
   */
  LeafNode *updateCell(LeafNode *node, CubeTriangleIsect *p);

  /* Routines to detect and patch holes */
  int numRings;
  int totRingLengths;
  int maxRingLength;

  /**
   * Entry routine.
   */
  void trace();
  /**
   * Trace the given node, find patches and fill them in
   */
  Node *trace(Node *node, int *st, int len, int depth, PathList *&paths);
  /**
   * Look for path on the face and add to paths
   */
  void findPaths(
      Node *node[2], int leaf[2], int depth[2], int *st[2], int maxdep, int dir, PathList *&paths);
  /**
   * Combine two list1 and list2 into list1 using connecting paths list3,
   * while closed paths are appended to rings
   */
  void combinePaths(PathList *&list1, PathList *list2, PathList *paths, PathList *&rings);
  /**
   * Helper function: combine current paths in list1 and list2 to a single path and append to list3
   */
  PathList *combineSinglePath(PathList *&head1,
                              PathList *pre1,
                              PathList *&list1,
                              PathList *&head2,
                              PathList *pre2,
                              PathList *&list2);

  /**
   * Functions to patch rings in a node
   */
  Node *patch(Node *node, int st[3], int len, PathList *rings);
  Node *patchSplit(Node *node,
                   int st[3],
                   int len,
                   PathList *rings,
                   int dir,
                   PathList *&nrings1,
                   PathList *&nrings2);
  Node *patchSplitSingle(Node *node,
                         int st[3],
                         int len,
                         PathElement *head,
                         int dir,
                         PathList *&nrings1,
                         PathList *&nrings2);
  Node *connectFace(Node *node, int st[3], int len, int dir, PathElement *f1, PathElement *f2);
  Node *locateCell(InternalNode *node,
                   int st[3],
                   int len,
                   int ori[3],
                   int dir,
                   int side,
                   Node *&rleaf,
                   int rst[3],
                   int &rlen);
  void compressRing(PathElement *&ring);
  void getFacePoint(PathElement *leaf, int dir, int &x, int &y, float &p, float &q);
  LeafNode *patchAdjacent(InternalNode *node,
                          int len,
                          int st1[3],
                          LeafNode *leaf1,
                          int st2[3],
                          LeafNode *leaf2,
                          int walkdir,
                          int inc,
                          int dir,
                          int side,
                          float alpha);
  int findPair(PathElement *head, int pos, int dir, PathElement *&pre1, PathElement *&pre2);
  int getSide(PathElement *e, int pos, int dir);
  int isEqual(PathElement *e1, PathElement *e2);
  void preparePrimalEdgesMask(InternalNode *node);
  void testFacePoint(PathElement *e1, PathElement *e2);

  /**
   * Path-related functions
   */
  void deletePath(PathList *&head, PathList *pre, PathList *&curr);
  void printPath(PathList *path);
  void printPath(PathElement *path);
  void printElement(PathElement *ele);
  void printPaths(PathList *path);
  void checkElement(PathElement *ele);
  void checkPath(PathElement *path);

  /**
   * Routines to build signs to create a partitioned volume
   *(after patching rings)
   */
  void buildSigns();
  void buildSigns(unsigned char table[], Node *node, int isLeaf, int sg, int rvalue[8]);

  /************************************************************************/
  /* To remove disconnected components */
  /************************************************************************/
  void floodFill();
  void clearProcessBits(Node *node, int height);
  int floodFill(LeafNode *leaf, int st[3], int len, int height, int threshold);
  int floodFill(Node *node, int st[3], int len, int height, int threshold);

  /**
   * Write out polygon file
   */
  void writeOut();

  void countIntersection(Node *node, int height, int &nedge, int &ncell, int &nface);
  void generateMinimizer(Node *node, int st[3], int len, int height, int &offset);
  void computeMinimizer(const LeafNode *leaf, int st[3], int len, float rvalue[3]) const;
  /**
   * Traversal functions to generate polygon model
   * op: 0 for counting, 1 for writing OBJ, 2 for writing OFF, 3 for writing PLY
   */
  void cellProcContour(Node *node, int leaf, int depth);
  void faceProcContour(Node *node[2], int leaf[2], int depth[2], int maxdep, int dir);
  void edgeProcContour(Node *node[4], int leaf[4], int depth[4], int maxdep, int dir);
  void processEdgeWrite(Node *node[4], int depths[4], int maxdep, int dir);

  /* output callbacks/data */
  DualConAllocOutput alloc_output;
  DualConAddVert add_vert;
  DualConAddQuad add_quad;
  void *output_mesh;

 private:
  /************ Operators for all nodes ************/

  /// Lookup table
  int numEdgeTable[8];
  int edgeCountTable[8][3];

  /// Build up lookup table
  void buildTable()
  {
    for (int i = 0; i < 256; i++) {
      InternalNode::numChildrenTable[i] = 0;
      int count = 0;
      for (int j = 0; j < 8; j++) {
        InternalNode::numChildrenTable[i] += ((i >> j) & 1);
        InternalNode::childrenCountTable[i][j] = count;
        InternalNode::childrenIndexTable[i][count] = j;
        count += ((i >> j) & 1);
      }
    }

    for (int i = 0; i < 8; i++) {
      numEdgeTable[i] = 0;
      int count = 0;
      for (int j = 0; j < 3; j++) {
        numEdgeTable[i] += ((i >> j) & 1);
        edgeCountTable[i][j] = count;
        count += ((i >> j) & 1);
      }
    }
  }

  int getSign(Node *node, int height, int index)
  {
    if (height == 0) {
      return getSign(&node->leaf, index);
    }
    else {
      if (node->internal.has_child(index)) {
        return getSign(
            node->internal.get_child(node->internal.get_child_count(index)), height - 1, index);
      }
      else {
        return getSign(
            node->internal.get_child(0), height - 1, 7 - node->internal.get_child_index(0));
      }
    }
  }

  /************ Operators for leaf nodes ************/

  void printInfo(int st[3])
  {
    printf("INFO AT: %d %d %d\n", st[0] >> minshift, st[1] >> minshift, st[2] >> minshift);
    LeafNode *leaf = (LeafNode *)locateLeafCheck(st);
    if (leaf)
      printInfo(leaf);
    else
      printf("Leaf not exists!\n");
  }

  void printInfo(const LeafNode *leaf)
  {
    /*
      printf("Edge mask: ");
      for(int i = 0; i < 12; i ++)
      {
      printf("%d ", getEdgeParity(leaf, i));
      }
      printf("\n");
      printf("Stored edge mask: ");
      for(i = 0; i < 3; i ++)
      {
      printf("%d ", getStoredEdgesParity(leaf, i));
      }
      printf("\n");
    */
    printf("Sign mask: ");
    for (int i = 0; i < 8; i++) {
      printf("%d ", getSign(leaf, i));
    }
    printf("\n");
  }

  /// Retrieve signs
  int getSign(const LeafNode *leaf, int index)
  {
    return ((leaf->signs >> index) & 1);
  }

  /// Set sign
  void setSign(LeafNode *leaf, int index)
  {
    leaf->signs |= (1 << index);
  }

  void setSign(LeafNode *leaf, int index, int sign)
  {
    leaf->signs &= (~(1 << index));
    leaf->signs |= ((sign & 1) << index);
  }

  int getSignMask(const LeafNode *leaf) const
  {
    return leaf->signs;
  }

  void setInProcessAll(int st[3], int dir)
  {
    int nst[3], eind;
    for (int i = 0; i < 4; i++) {
      nst[0] = st[0] + dirCell[dir][i][0] * mindimen;
      nst[1] = st[1] + dirCell[dir][i][1] * mindimen;
      nst[2] = st[2] + dirCell[dir][i][2] * mindimen;
      eind = dirEdge[dir][i];

      LeafNode *cell = locateLeafCheck(nst);
      assert(cell);

      setInProcess(cell, eind);
    }
  }

  void flipParityAll(int st[3], int dir)
  {
    int nst[3], eind;
    for (int i = 0; i < 4; i++) {
      nst[0] = st[0] + dirCell[dir][i][0] * mindimen;
      nst[1] = st[1] + dirCell[dir][i][1] * mindimen;
      nst[2] = st[2] + dirCell[dir][i][2] * mindimen;
      eind = dirEdge[dir][i];

      LeafNode *cell = locateLeaf(nst);
      flipEdge(cell, eind);
    }
  }

  void setInProcess(LeafNode *leaf, int eind)
  {
    assert(eind >= 0 && eind <= 11);

    leaf->flood_fill |= (1 << eind);
  }

  void setOutProcess(LeafNode *leaf, int eind)
  {
    assert(eind >= 0 && eind <= 11);

    leaf->flood_fill &= ~(1 << eind);
  }

  int isInProcess(LeafNode *leaf, int eind)
  {
    assert(eind >= 0 && eind <= 11);

    return (leaf->flood_fill >> eind) & 1;
  }

  /// Generate signs at the corners from the edge parity
  void generateSigns(LeafNode *leaf, unsigned char table[], int start)
  {
    leaf->signs = table[leaf->edge_parity];

    if ((start ^ leaf->signs) & 1) {
      leaf->signs = ~(leaf->signs);
    }
  }

  /// Get edge parity
  int getEdgeParity(const LeafNode *leaf, int index) const
  {
    assert(index >= 0 && index <= 11);

    return (leaf->edge_parity >> index) & 1;
  }

  /// Get edge parity on a face
  int getFaceParity(LeafNode *leaf, int index)
  {
    int a = getEdgeParity(leaf, faceMap[index][0]) + getEdgeParity(leaf, faceMap[index][1]) +
            getEdgeParity(leaf, faceMap[index][2]) + getEdgeParity(leaf, faceMap[index][3]);
    return (a & 1);
  }
  int getFaceEdgeNum(LeafNode *leaf, int index)
  {
    int a = getEdgeParity(leaf, faceMap[index][0]) + getEdgeParity(leaf, faceMap[index][1]) +
            getEdgeParity(leaf, faceMap[index][2]) + getEdgeParity(leaf, faceMap[index][3]);
    return a;
  }

  /// Set edge parity
  void flipEdge(LeafNode *leaf, int index)
  {
    assert(index >= 0 && index <= 11);

    leaf->edge_parity ^= (1 << index);
  }

  /// Set 1
  void setEdge(LeafNode *leaf, int index)
  {
    assert(index >= 0 && index <= 11);

    leaf->edge_parity |= (1 << index);
  }

  /// Set 0
  void resetEdge(LeafNode *leaf, int index)
  {
    assert(index >= 0 && index <= 11);

    leaf->edge_parity &= ~(1 << index);
  }

  /// Flipping with a new intersection offset
  void createPrimalEdgesMask(LeafNode *leaf)
  {
    leaf->primary_edge_intersections = getPrimalEdgesMask2(leaf);
  }

  void setStoredEdgesParity(LeafNode *leaf, int pindex)
  {
    assert(pindex <= 2 && pindex >= 0);

    leaf->primary_edge_intersections |= (1 << pindex);
  }

  int getStoredEdgesParity(const LeafNode *leaf, int pindex) const
  {
    assert(pindex <= 2 && pindex >= 0);

    return (leaf->primary_edge_intersections >> pindex) & 1;
  }

  LeafNode *flipEdge(LeafNode *leaf, int index, float alpha)
  {
    flipEdge(leaf, index);

    if ((index & 3) == 0) {
      int ind = index / 4;
      if (getEdgeParity(leaf, index) && !getStoredEdgesParity(leaf, ind)) {
        // Create a new node
        int num = getNumEdges(leaf) + 1;
        setStoredEdgesParity(leaf, ind);
        int count = getEdgeCount(leaf, ind);
        LeafNode *nleaf = createLeaf(num);
        *nleaf = *leaf;

        setEdgeOffset(nleaf, alpha, count);

        if (num > 1) {
          float *pts = leaf->edge_intersections;
          float *npts = nleaf->edge_intersections;
          for (int i = 0; i < count; i++) {
            for (int j = 0; j < EDGE_FLOATS; j++) {
              npts[i * EDGE_FLOATS + j] = pts[i * EDGE_FLOATS + j];
            }
          }
          for (int i = count + 1; i < num; i++) {
            for (int j = 0; j < EDGE_FLOATS; j++) {
              npts[i * EDGE_FLOATS + j] = pts[(i - 1) * EDGE_FLOATS + j];
            }
          }
        }

        removeLeaf(num - 1, (LeafNode *)leaf);
        leaf = nleaf;
      }
    }

    return leaf;
  }

  /// Update parent link
  void updateParent(InternalNode *node, int len, int st[3], LeafNode *leaf)
  {
    // First, locate the parent
    int count;
    InternalNode *parent = locateParent(node, len, st, count);

    // Update
    parent->set_child(count, (Node *)leaf);
  }

  void updateParent(InternalNode *node, int len, int st[3])
  {
    if (len == dimen) {
      root = (Node *)node;
      return;
    }

    // First, locate the parent
    int count;
    InternalNode *parent = locateParent(len, st, count);

    // UPdate
    parent->set_child(count, (Node *)node);
  }

  /// Find edge intersection on a given edge
  int getEdgeIntersectionByIndex(int st[3], int index, float pt[3], int check) const
  {
    // First, locat the leaf
    const LeafNode *leaf;
    if (check) {
      leaf = locateLeafCheck(st);
    }
    else {
      leaf = locateLeaf(st);
    }

    if (leaf && getStoredEdgesParity(leaf, index)) {
      float off = getEdgeOffset(leaf, getEdgeCount(leaf, index));
      pt[0] = (float)st[0];
      pt[1] = (float)st[1];
      pt[2] = (float)st[2];
      pt[index] += off * mindimen;

      return 1;
    }
    else {
      return 0;
    }
  }

  /// Retrieve number of edges intersected
  int getPrimalEdgesMask(const LeafNode *leaf) const
  {
    return leaf->primary_edge_intersections;
  }

  int getPrimalEdgesMask2(LeafNode *leaf)
  {
    return (((leaf->edge_parity & 0x1) >> 0) | ((leaf->edge_parity & 0x10) >> 3) |
            ((leaf->edge_parity & 0x100) >> 6));
  }

  /// Get the count for a primary edge
  int getEdgeCount(const LeafNode *leaf, int index) const
  {
    return edgeCountTable[getPrimalEdgesMask(leaf)][index];
  }
  int getNumEdges(LeafNode *leaf)
  {
    return numEdgeTable[getPrimalEdgesMask(leaf)];
  }

  int getNumEdges2(LeafNode *leaf)
  {
    return numEdgeTable[getPrimalEdgesMask2(leaf)];
  }

  /// Set edge intersection
  void setEdgeOffset(LeafNode *leaf, float pt, int count)
  {
    float *pts = leaf->edge_intersections;
    pts[EDGE_FLOATS * count] = pt;
    pts[EDGE_FLOATS * count + 1] = 0;
    pts[EDGE_FLOATS * count + 2] = 0;
    pts[EDGE_FLOATS * count + 3] = 0;
  }

  /// Set multiple edge intersections
  void setEdgeOffsets(LeafNode *leaf, float pt[3], int len)
  {
    float *pts = leaf->edge_intersections;
    for (int i = 0; i < len; i++) {
      pts[i] = pt[i];
    }
  }

  /// Retrieve edge intersection
  float getEdgeOffset(const LeafNode *leaf, int count) const
  {
    return leaf->edge_intersections[4 * count];
  }

  /// Update method
  LeafNode *updateEdgeOffsets(LeafNode *leaf, int oldlen, int newlen, float offs[3])
  {
    // First, create a new leaf node
    LeafNode *nleaf = createLeaf(newlen);
    *nleaf = *leaf;

    // Next, fill in the offsets
    setEdgeOffsets(nleaf, offs, newlen);

    // Finally, delete the old leaf
    removeLeaf(oldlen, leaf);

    return nleaf;
  }

  /// Set minimizer index
  void setMinimizerIndex(LeafNode *leaf, int index)
  {
    leaf->minimizer_index = index;
  }

  /// Get minimizer index
  int getMinimizerIndex(LeafNode *leaf)
  {
    return leaf->minimizer_index;
  }

  int getMinimizerIndex(LeafNode *leaf, int eind)
  {
    int add = manifold_table[getSignMask(leaf)].pairs[eind][0] - 1;
    assert(add >= 0);
    return leaf->minimizer_index + add;
  }

  void getMinimizerIndices(LeafNode *leaf, int eind, int inds[2])
  {
    const int *add = manifold_table[getSignMask(leaf)].pairs[eind];
    inds[0] = leaf->minimizer_index + add[0] - 1;
    if (add[0] == add[1]) {
      inds[1] = -1;
    }
    else {
      inds[1] = leaf->minimizer_index + add[1] - 1;
    }
  }

  /// Set edge intersection
  void setEdgeOffsetNormal(LeafNode *leaf, float pt, float a, float b, float c, int count)
  {
    float *pts = leaf->edge_intersections;
    pts[4 * count] = pt;
    pts[4 * count + 1] = a;
    pts[4 * count + 2] = b;
    pts[4 * count + 3] = c;
  }

  float getEdgeOffsetNormal(LeafNode *leaf, int count, float &a, float &b, float &c)
  {
    float *pts = leaf->edge_intersections;
    a = pts[4 * count + 1];
    b = pts[4 * count + 2];
    c = pts[4 * count + 3];
    return pts[4 * count];
  }

  /// Set multiple edge intersections
  void setEdgeOffsetsNormals(
      LeafNode *leaf, const float pt[], const float a[], const float b[], const float c[], int len)
  {
    float *pts = leaf->edge_intersections;
    for (int i = 0; i < len; i++) {
      if (pt[i] > 1 || pt[i] < 0) {
        printf("\noffset: %f\n", pt[i]);
      }
      pts[i * 4] = pt[i];
      pts[i * 4 + 1] = a[i];
      pts[i * 4 + 2] = b[i];
      pts[i * 4 + 3] = c[i];
    }
  }

  /// Retrieve complete edge intersection
  void getEdgeIntersectionByIndex(
      const LeafNode *leaf, int index, int st[3], int len, float pt[3], float nm[3]) const
  {
    int count = getEdgeCount(leaf, index);
    const float *pts = leaf->edge_intersections;

    float off = pts[4 * count];

    pt[0] = (float)st[0];
    pt[1] = (float)st[1];
    pt[2] = (float)st[2];
    pt[index] += (off * len);

    nm[0] = pts[4 * count + 1];
    nm[1] = pts[4 * count + 2];
    nm[2] = pts[4 * count + 3];
  }

  float getEdgeOffsetNormalByIndex(LeafNode *leaf, int index, float nm[3])
  {
    int count = getEdgeCount(leaf, index);
    float *pts = leaf->edge_intersections;

    float off = pts[4 * count];

    nm[0] = pts[4 * count + 1];
    nm[1] = pts[4 * count + 2];
    nm[2] = pts[4 * count + 3];

    return off;
  }

  void fillEdgeIntersections(
      const LeafNode *leaf, int st[3], int len, float pts[12][3], float norms[12][3]) const
  {
    int i;
    // int stt[3] = {0, 0, 0};

    // The three primal edges are easy
    int pmask[3] = {0, 4, 8};
    for (i = 0; i < 3; i++) {
      if (getEdgeParity(leaf, pmask[i])) {
        // getEdgeIntersectionByIndex(leaf, i, stt, 1, pts[pmask[i]], norms[pmask[i]]);
        getEdgeIntersectionByIndex(leaf, i, st, len, pts[pmask[i]], norms[pmask[i]]);
      }
    }

    // 3 face adjacent cubes
    int fmask[3][2] = {{6, 10}, {2, 9}, {1, 5}};
    int femask[3][2] = {{1, 2}, {0, 2}, {0, 1}};
    for (i = 0; i < 3; i++) {
      int e1 = getEdgeParity(leaf, fmask[i][0]);
      int e2 = getEdgeParity(leaf, fmask[i][1]);
      if (e1 || e2) {
        int nst[3] = {st[0], st[1], st[2]};
        nst[i] += len;
        // int nstt[3] = {0, 0, 0};
        // nstt[i] += 1;
        const LeafNode *node = locateLeaf(nst);

        if (e1) {
          // getEdgeIntersectionByIndex(node, femask[i][0], nstt, 1, pts[fmask[i][0]],
          // norms[fmask[i][0]]);
          getEdgeIntersectionByIndex(
              node, femask[i][0], nst, len, pts[fmask[i][0]], norms[fmask[i][0]]);
        }
        if (e2) {
          // getEdgeIntersectionByIndex(node, femask[i][1], nstt, 1, pts[fmask[i][1]],
          // norms[fmask[i][1]]);
          getEdgeIntersectionByIndex(
              node, femask[i][1], nst, len, pts[fmask[i][1]], norms[fmask[i][1]]);
        }
      }
    }

    // 3 edge adjacent cubes
    int emask[3] = {3, 7, 11};
    int eemask[3] = {0, 1, 2};
    for (i = 0; i < 3; i++) {
      if (getEdgeParity(leaf, emask[i])) {
        int nst[3] = {st[0] + len, st[1] + len, st[2] + len};
        nst[i] -= len;
        // int nstt[3] = {1, 1, 1};
        // nstt[i] -= 1;
        const LeafNode *node = locateLeaf(nst);

        // getEdgeIntersectionByIndex(node, eemask[i], nstt, 1, pts[emask[i]], norms[emask[i]]);
        getEdgeIntersectionByIndex(node, eemask[i], nst, len, pts[emask[i]], norms[emask[i]]);
      }
    }
  }

  void fillEdgeIntersections(const LeafNode *leaf,
                             int st[3],
                             int len,
                             float pts[12][3],
                             float norms[12][3],
                             int parity[12]) const
  {
    int i;
    for (i = 0; i < 12; i++) {
      parity[i] = 0;
    }
    // int stt[3] = {0, 0, 0};

    // The three primal edges are easy
    int pmask[3] = {0, 4, 8};
    for (i = 0; i < 3; i++) {
      if (getStoredEdgesParity(leaf, i)) {
        // getEdgeIntersectionByIndex(leaf, i, stt, 1, pts[pmask[i]], norms[pmask[i]]);
        getEdgeIntersectionByIndex(leaf, i, st, len, pts[pmask[i]], norms[pmask[i]]);
        parity[pmask[i]] = 1;
      }
    }

    // 3 face adjacent cubes
    int fmask[3][2] = {{6, 10}, {2, 9}, {1, 5}};
    int femask[3][2] = {{1, 2}, {0, 2}, {0, 1}};
    for (i = 0; i < 3; i++) {
      {
        int nst[3] = {st[0], st[1], st[2]};
        nst[i] += len;
        // int nstt[3] = {0, 0, 0};
        // nstt[i] += 1;
        const LeafNode *node = locateLeafCheck(nst);
        if (node == NULL) {
          continue;
        }

        int e1 = getStoredEdgesParity(node, femask[i][0]);
        int e2 = getStoredEdgesParity(node, femask[i][1]);

        if (e1) {
          // getEdgeIntersectionByIndex(node, femask[i][0], nstt, 1, pts[fmask[i][0]],
          // norms[fmask[i][0]]);
          getEdgeIntersectionByIndex(
              node, femask[i][0], nst, len, pts[fmask[i][0]], norms[fmask[i][0]]);
          parity[fmask[i][0]] = 1;
        }
        if (e2) {
          // getEdgeIntersectionByIndex(node, femask[i][1], nstt, 1, pts[fmask[i][1]],
          // norms[fmask[i][1]]);
          getEdgeIntersectionByIndex(
              node, femask[i][1], nst, len, pts[fmask[i][1]], norms[fmask[i][1]]);
          parity[fmask[i][1]] = 1;
        }
      }
    }

    // 3 edge adjacent cubes
    int emask[3] = {3, 7, 11};
    int eemask[3] = {0, 1, 2};
    for (i = 0; i < 3; i++) {
      //          if(getEdgeParity(leaf, emask[i]))
      {
        int nst[3] = {st[0] + len, st[1] + len, st[2] + len};
        nst[i] -= len;
        // int nstt[3] = {1, 1, 1};
        // nstt[i] -= 1;
        const LeafNode *node = locateLeafCheck(nst);
        if (node == NULL) {
          continue;
        }

        if (getStoredEdgesParity(node, eemask[i])) {
          // getEdgeIntersectionByIndex(node, eemask[i], nstt, 1, pts[emask[i]], norms[emask[i]]);
          getEdgeIntersectionByIndex(node, eemask[i], nst, len, pts[emask[i]], norms[emask[i]]);
          parity[emask[i]] = 1;
        }
      }
    }
  }

  void fillEdgeOffsetsNormals(
      LeafNode *leaf, int st[3], int len, float pts[12], float norms[12][3], int parity[12])
  {
    int i;
    for (i = 0; i < 12; i++) {
      parity[i] = 0;
    }
    // int stt[3] = {0, 0, 0};

    // The three primal edges are easy
    int pmask[3] = {0, 4, 8};
    for (i = 0; i < 3; i++) {
      if (getStoredEdgesParity(leaf, i)) {
        pts[pmask[i]] = getEdgeOffsetNormalByIndex(leaf, i, norms[pmask[i]]);
        parity[pmask[i]] = 1;
      }
    }

    // 3 face adjacent cubes
    int fmask[3][2] = {{6, 10}, {2, 9}, {1, 5}};
    int femask[3][2] = {{1, 2}, {0, 2}, {0, 1}};
    for (i = 0; i < 3; i++) {
      {
        int nst[3] = {st[0], st[1], st[2]};
        nst[i] += len;
        // int nstt[3] = {0, 0, 0};
        // nstt[i] += 1;
        LeafNode *node = locateLeafCheck(nst);
        if (node == NULL) {
          continue;
        }

        int e1 = getStoredEdgesParity(node, femask[i][0]);
        int e2 = getStoredEdgesParity(node, femask[i][1]);

        if (e1) {
          pts[fmask[i][0]] = getEdgeOffsetNormalByIndex(node, femask[i][0], norms[fmask[i][0]]);
          parity[fmask[i][0]] = 1;
        }
        if (e2) {
          pts[fmask[i][1]] = getEdgeOffsetNormalByIndex(node, femask[i][1], norms[fmask[i][1]]);
          parity[fmask[i][1]] = 1;
        }
      }
    }

    // 3 edge adjacent cubes
    int emask[3] = {3, 7, 11};
    int eemask[3] = {0, 1, 2};
    for (i = 0; i < 3; i++) {
      //          if(getEdgeParity(leaf, emask[i]))
      {
        int nst[3] = {st[0] + len, st[1] + len, st[2] + len};
        nst[i] -= len;
        // int nstt[3] = {1, 1, 1};
        // nstt[i] -= 1;
        LeafNode *node = locateLeafCheck(nst);
        if (node == NULL) {
          continue;
        }

        if (getStoredEdgesParity(node, eemask[i])) {
          pts[emask[i]] = getEdgeOffsetNormalByIndex(node, eemask[i], norms[emask[i]]);
          parity[emask[i]] = 1;
        }
      }
    }
  }

  /// Update method
  LeafNode *updateEdgeOffsetsNormals(
      LeafNode *leaf, int oldlen, int newlen, float offs[3], float a[3], float b[3], float c[3])
  {
    // First, create a new leaf node
    LeafNode *nleaf = createLeaf(newlen);
    *nleaf = *leaf;

    // Next, fill in the offsets
    setEdgeOffsetsNormals(nleaf, offs, a, b, c, newlen);

    // Finally, delete the old leaf
    removeLeaf(oldlen, leaf);

    return nleaf;
  }

  /// Locate a leaf
  /// WARNING: assuming this leaf already exists!

  LeafNode *locateLeaf(int st[3])
  {
    Node *node = (Node *)root;
    for (int i = GRID_DIMENSION - 1; i > GRID_DIMENSION - maxDepth - 1; i--) {
      int index = (((st[0] >> i) & 1) << 2) | (((st[1] >> i) & 1) << 1) | (((st[2] >> i) & 1));
      node = node->internal.get_child(node->internal.get_child_count(index));
    }

    return &node->leaf;
  }

  const LeafNode *locateLeaf(int st[3]) const
  {
    const Node *node = root;
    for (int i = GRID_DIMENSION - 1; i > GRID_DIMENSION - maxDepth - 1; i--) {
      int index = (((st[0] >> i) & 1) << 2) | (((st[1] >> i) & 1) << 1) | (((st[2] >> i) & 1));
      node = node->internal.get_child(node->internal.get_child_count(index));
    }

    return &node->leaf;
  }

  LeafNode *locateLeaf(InternalNode *parent, int len, int st[3])
  {
    Node *node = (Node *)parent;
    int index;
    for (int i = len / 2; i >= mindimen; i >>= 1) {
      index = (((st[0] & i) ? 4 : 0) | ((st[1] & i) ? 2 : 0) | ((st[2] & i) ? 1 : 0));
      node = node->internal.get_child(node->internal.get_child_count(index));
    }

    return &node->leaf;
  }

  LeafNode *locateLeafCheck(int st[3])
  {
    Node *node = (Node *)root;
    for (int i = GRID_DIMENSION - 1; i > GRID_DIMENSION - maxDepth - 1; i--) {
      int index = (((st[0] >> i) & 1) << 2) | (((st[1] >> i) & 1) << 1) | (((st[2] >> i) & 1));
      if (!node->internal.has_child(index)) {
        return NULL;
      }
      node = node->internal.get_child(node->internal.get_child_count(index));
    }

    return &node->leaf;
  }

  const LeafNode *locateLeafCheck(int st[3]) const
  {
    const Node *node = root;
    for (int i = GRID_DIMENSION - 1; i > GRID_DIMENSION - maxDepth - 1; i--) {
      int index = (((st[0] >> i) & 1) << 2) | (((st[1] >> i) & 1) << 1) | (((st[2] >> i) & 1));
      if (!node->internal.has_child(index)) {
        return NULL;
      }
      node = node->internal.get_child(node->internal.get_child_count(index));
    }

    return &node->leaf;
  }

  InternalNode *locateParent(int len, int st[3], int &count)
  {
    InternalNode *node = (InternalNode *)root;
    InternalNode *pre = NULL;
    int index = 0;
    for (int i = dimen / 2; i >= len; i >>= 1) {
      index = (((st[0] & i) ? 4 : 0) | ((st[1] & i) ? 2 : 0) | ((st[2] & i) ? 1 : 0));
      pre = node;
      node = &node->get_child(node->get_child_count(index))->internal;
    }

    count = pre->get_child_count(index);
    return pre;
  }

  InternalNode *locateParent(InternalNode *parent, int len, int st[3], int &count)
  {
    InternalNode *node = parent;
    InternalNode *pre = NULL;
    int index = 0;
    for (int i = len / 2; i >= mindimen; i >>= 1) {
      index = (((st[0] & i) ? 4 : 0) | ((st[1] & i) ? 2 : 0) | ((st[2] & i) ? 1 : 0));
      pre = node;
      node = &node->get_child(node->get_child_count(index))->internal;
    }

    count = pre->get_child_count(index);
    return pre;
  }

  /************ Operators for internal nodes ************/

  /// Add a kid to an existing internal node
  InternalNode *addChild(InternalNode *node, int index, Node *child, int aLeaf)
  {
    // Create new internal node
    int num = node->get_num_children();
    InternalNode *rnode = createInternal(num + 1);

    // Establish children
    int i;
    int count1 = 0, count2 = 0;
    for (i = 0; i < 8; i++) {
      if (i == index) {
        if (aLeaf) {
          rnode->set_leaf_child(i, count2, &child->leaf);
        }
        else {
          rnode->set_internal_child(i, count2, &child->internal);
        }
        count2++;
      }
      else if (node->has_child(i)) {
        if (node->is_child_leaf(i)) {
          rnode->set_leaf_child(i, count2, &node->get_child(count1)->leaf);
        }
        else {
          rnode->set_internal_child(i, count2, &node->get_child(count1)->internal);
        }
        count1++;
        count2++;
      }
    }

    removeInternal(num, node);
    return rnode;
  }

  /// Allocate a node
  InternalNode *createInternal(int length)
  {
    InternalNode *inode = (InternalNode *)alloc[length]->allocate();
    inode->has_child_bitfield = 0;
    inode->child_is_leaf_bitfield = 0;
    return inode;
  }

  LeafNode *createLeaf(int length)
  {
    assert(length <= 3);

    LeafNode *lnode = (LeafNode *)leafalloc[length]->allocate();
    lnode->edge_parity = 0;
    lnode->primary_edge_intersections = 0;
    lnode->signs = 0;

    return lnode;
  }

  void removeInternal(int num, InternalNode *node)
  {
    alloc[num]->deallocate(node);
  }

  void removeLeaf(int num, LeafNode *leaf)
  {
    assert(num >= 0 && num <= 3);
    leafalloc[num]->deallocate(leaf);
  }

  /// Add a leaf (by creating a new par node with the leaf added)
  InternalNode *addLeafChild(InternalNode *par, int index, int count, LeafNode *leaf)
  {
    int num = par->get_num_children() + 1;
    InternalNode *npar = createInternal(num);
    *npar = *par;

    if (num == 1) {
      npar->set_leaf_child(index, 0, leaf);
    }
    else {
      int i;
      for (i = 0; i < count; i++) {
        npar->set_child(i, par->get_child(i));
      }
      npar->set_leaf_child(index, count, leaf);
      for (i = count + 1; i < num; i++) {
        npar->set_child(i, par->get_child(i - 1));
      }
    }

    removeInternal(num - 1, par);
    return npar;
  }

  InternalNode *addInternalChild(InternalNode *par, int index, int count, InternalNode *node)
  {
    int num = par->get_num_children() + 1;
    InternalNode *npar = createInternal(num);
    *npar = *par;

    if (num == 1) {
      npar->set_internal_child(index, 0, node);
    }
    else {
      int i;
      for (i = 0; i < count; i++) {
        npar->set_child(i, par->get_child(i));
      }
      npar->set_internal_child(index, count, node);
      for (i = count + 1; i < num; i++) {
        npar->set_child(i, par->get_child(i - 1));
      }
    }

    removeInternal(num - 1, par);
    return npar;
  }

#ifdef WITH_CXX_GUARDEDALLOC
  MEM_CXX_CLASS_ALLOC_FUNCS("DUALCON:Octree")
#endif
};

#endif /* __OCTREE_H__ */
